package com.example

import scala.annotation.tailrec
import scala.collection.mutable
import scala.math.{pow, sqrt}

object AStar {
  // 路径节点，用于A*算法
  case class PathNode(treeNode: Leaf,
                      parent: Option[PathNode],
                      g: Double, // 从起点到当前节点的实际代价
                      h: Double) // 到终点的启发式估计代价
  {
    def f: Double = g + h // 总代价
  }

  // 计算两个节点中心点之间的欧几里得距离
  private def distance(a: Leaf, b: Leaf): Double = {
    val (ax, ay) = a.center
    val (bx, by) = b.center
    sqrt(pow(ax - bx, 2) + pow(ay - by, 2))
  }

  // 查找包含指定坐标的叶子节点
  private def findLeafAt(root: QuadTree, x: Int, y: Int): Option[Leaf] = {
    def contains(t: QuadTree): Boolean = t match {
      case l: Leaf => x >= l.bound.x && x < l.bound.x + l.bound.size && y >= l.bound.y && y < l.bound.y + l.bound.size
      case m: Node => x >= m.bound.x && x < m.bound.x + m.bound.size && y >= m.bound.y && y < m.bound.y + m.bound.size
      case _ => false
    }

    def search(node: QuadTree): Option[Leaf] = node match {
      case l: Leaf if contains(l) => Some(l)
      case n: Node if contains(n) =>
        Array(n.nw, n.ne, n.sw, n.se).flatMap(search).headOption
      case _ => None
    }

    search(root)
  }

  // A*路径查找主函数
  def findPath(root: QuadTree, startX: Int, startY: Int, endX: Int, endY: Int): Option[List[(Int, Int)]] = {
    // 查找起点和终点所在的叶子节点
    val startNode = findLeafAt(root, startX, startY)
    val endNode = findLeafAt(root, endX, endY)

    (startNode, endNode) match {
      case (Some(start), Some(end)) if start.state == Empty && end.state == Empty =>
        // 初始化开放集和关闭集
        val openSet = mutable.PriorityQueue.empty[PathNode](Ordering.by(-_.f))
        val closedSet = mutable.Set.empty[QuadTree]
        val nodeMap = mutable.Map.empty[QuadTree, PathNode] // 用于快速查找

        // 创建起点PathNode
        val initialH = distance(start, end)
        val initialNode = PathNode(start, None, 0, initialH)
        openSet.enqueue(initialNode)
        nodeMap(start) = initialNode

        // A*主循环
        var found = false
        var result: Option[PathNode] = None

        while (openSet.nonEmpty && !found) {
          val current = openSet.dequeue()

          if (current.treeNode == end) {
            found = true
            result = Some(current)
          } else {
            closedSet.add(current.treeNode)

            // 检查所有邻居
            for (neighbor <- current.treeNode.neighbors if !closedSet.contains(neighbor)) {
              val tentativeG = current.g + distance(current.treeNode, neighbor)

              nodeMap.get(neighbor) match {
                case Some(existing) if tentativeG >= existing.g =>
                  () // 不是更好的路径
                case _ =>
                  // 新节点或找到更好路径
                  val h = distance(neighbor, end)
                  val newNode = PathNode(neighbor, Some(current), tentativeG, h)
                  openSet.enqueue(newNode)
                  nodeMap(neighbor) = newNode
              }
            }
          }
        }
        // 重建路径
        result.map(reconstructPath)
      case _ => None // 起点或终点不可达
    }
  }

  // 从终点回溯重建路径
  private def reconstructPath(endNode: PathNode): List[(Int, Int)] = {
    @tailrec
    def buildPath(node: PathNode, acc: List[(Int, Int)]): List[(Int, Int)] = {
      node.parent match {
        case Some(parent) => buildPath(parent, node.treeNode.center :: acc)
        case None => node.treeNode.center :: acc
      }
    }
    buildPath(endNode, Nil)
  }

//  def main(args: Array[String]): Unit = {
//    println("You are now in Astar main()")
//  }
}
